/* clock em Hz */
#define F_CPU 16000000UL
#define P_SIN 2.0 // SIN OUTPUT PERIOD (in seconds)
#define P_SERVO 20.0 // PWM SERVO PERIOD (ms)

#include <math.h>
#include <avr/io.h>
#include <util/delay.h>

void init_pwm(void) {

	TCCR1A = _BV(WGM11) |  _BV(COM1A1);
	TCCR1B = _BV(CS11) | _BV(WGM12) | _BV(WGM13); /* prescaler of 8, fast PWM */

        // 3456 is the servo center position
        OCR1A = 1500; /* 125 - 250 */
        ICR1 = 19999; // that evens out to 50hz

        DDRB = _BV(PB1); /* Set pin as output */
}

int my_sin(uint16_t k)
{
  return ((int) 500*sin(((double) k)*2.0*M_PI/(P_SIN/P_SERVO*1000)));
}

int main(void) {

  uint16_t k=0;

  /* set PB0 to output */
  DDRC |= 1 << PC5;
  init_pwm();
  _delay_ms(1000);

  while(1) {
    OCR1A = 1750 + my_sin(k);
    _delay_ms(P_SERVO);
    k++;
    if (k == P_SIN/P_SERVO*1000) k = 0;

    if (OCR1A>1750) {
      PORTC |= 1 << PC5;
    }
    else {
      PORTC &= ~(1 << PC5);
    }
  }
	return 0;
}
